﻿using UnityEngine;
using System.Collections;

public class Triangulation : MonoBehaviour 
{
    public Beacon[] beacons;
    public bool isReal = false;

    void Update()
    {
        if (!isReal)
        {
            // 1. compute the modified beacon coordinates:
            Vector2 p1 = beacons[0].transform.position;
            Vector2 p2 = beacons[1].transform.position;
            Vector2 p3 = beacons[2].transform.position;

            Vector2 p_1 = p1 - p2;
            Vector2 p_3 = p3 - p2;

            // 2. compute the three cot(.):
            float a1 = CalcAngel(transform.position, beacons[0].transform.position);
            float a2 = CalcAngel(transform.position, beacons[1].transform.position);
            float a3 = CalcAngel(transform.position, beacons[2].transform.position);

            float T12 = 1f / Mathf.Tan(a2 - a1);
            float T23 = 1f / Mathf.Tan(a3 - a2);
            float T31 = (1f - T12 * T23) / (T12 + T23);

            // 3. compute the modified circle center coordinates {x’ij,  y’ij}:
            Vector2 p_12 = new Vector2(p_1.x + T12 * p_1.y, p_1.y + T12 * p_1.x);
            Vector2 p_23 = new Vector2(p_3.x + T23 * p_3.y, p_3.y + T23 * p_3.x);
            Vector2 p_31 = new Vector2(p_3.x + p_1.x + T31 * (p_3.y - p_1.y), p_3.y + p_1.y + T31 * (p_3.x - p_1.x));

            // 4. compute k’31:
            float k_31 = p_1.x * p_3.x + p_1.y * p_3.y + T31 * (p_1.x * p_3.y - p_3.x * p_1.y); // v

            // 5. compute the denominator D (if D = 0, return with an error):
            float D = (p_12.x - p_23.x) * (p_23.y - p_31.y) - (p_12.y - p_23.y) * (p_23.x - p_31.x); // v

            // 6. compute the robot position {xR,  yR} and return:
            Vector2 trianglePos = new Vector2(p2.x + (k_31 * (p_12.y - p_23.y)) / D, // v
                p2.y + (k_31 * (p_12.x - p_23.x)) / D);

            print(trianglePos);
        }
    }

    float CalcAngel(Vector2 p1, Vector2 p2)
    {
        Vector2 diff = p2 - p1;
        return Mathf.Atan2(diff.y, diff.x) * 180.0f / Mathf.PI;
    }

}
